/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.SpeedController;

/**
 * methods for driving the robot
 * @author robert
 */
public class PhoenixDrive {
    public SpeedController frontLeftMotor;
    public SpeedController rearLeftMotor;
    public SpeedController frontRightMotor;
    public SpeedController rearRightMotor;
    public Encoder leftEncoder;
    public Encoder rightEncoder;
    Const constants;

    /**
     * initialize robot drive with four CAN jaguars
     * @param frontLeftMotor front left motor
     * @param rearLeftMotor rear left motor
     * @param frontRightMotor front right motor
     * @param rearRightMotor rear right motor
     * @param leftEncoder1 left encoder port 1
     * @param leftEncoder2 left encoder port 2
     * @param rightEncoder1 right encoder port 1
     * @param rightEncoder2 right encoder port 2
     */
    public PhoenixDrive(SpeedController frontLeftMotor,
            SpeedController rearLeftMotor, SpeedController frontRightMotor,
            SpeedController rearRightMotor, Encoder leftEncoder,
            Encoder rightEncoder, Const constants) {
        this.frontLeftMotor = frontLeftMotor;
        this.rearLeftMotor = rearLeftMotor;
        this.frontRightMotor = frontRightMotor;
        this.rearRightMotor = rearRightMotor;
        this.leftEncoder = leftEncoder;
        this.rightEncoder = rightEncoder;
        this.constants = constants;
    }

    /**
     * sets speed for left side
     * @param speed
     */
    public void setLeft(double speed) {
        frontLeftMotor.set(speed * constants.LEFTINVERTED());
        if(rearLeftMotor != null)
            rearLeftMotor.set(speed * constants.LEFTINVERTED());
    }

    /**
     * sets speed for right side
     * @param speed
     */
    public void setRight(double speed) {     
        frontRightMotor.set(speed * constants.RIGHTINVERTED());
        if(rearRightMotor != null) 
            rearRightMotor.set(speed * constants.RIGHTINVERTED());
    }

    /**
     * @brief standard tank drive
     * @param leftSpeed
     * @param rightSpeed
     */
    public void tankDrive (double leftSpeed, double rightSpeed) {
        setLeft(leftSpeed);
        setRight(rightSpeed);
    }

    /**
     * @brief return average speed values for left motors
     * @return average left motor values
     */
    public double getPowerLeft() {
        double frontValue = 0;
        double rearValue = 0;

        // get value of front and rear motors
        frontValue = frontLeftMotor.get();
        if(rearLeftMotor != null) rearValue = rearLeftMotor.get();

        // return average of front and rear values
        return (frontValue + rearValue) / 2;
    }

    /**
     * @brief returns average speed values for right motors
     * @return average right motor values
     */
    public double getPowerRight() {
        double frontValue = 0;
        double rearValue = 0;

        // get value of front and rear motors
        frontValue = frontRightMotor.get();
        if(rearRightMotor != null) rearValue = rearRightMotor.get();

        // return average of front and rear values
        return (frontValue + rearValue) / 2;
    }

    /**
     * @brief resets and starts encoders
     */
    public void encoderStart() {
        if(leftEncoder != null) leftEncoder.reset();
        rightEncoder.reset();
        if(leftEncoder != null) leftEncoder.start();
        rightEncoder.start();
    }

    /**
     * @brief gets value of left encoder
     * @return left encoder value
     */
    public long getEncoderLeft() {
        return (leftEncoder != null ? leftEncoder.get() : 0);
    }

    /**
     * @brief gets value of right encoder
     * @return right encoder value
     */
    public long getEncoderRight() {
        return rightEncoder.get() * -1;
    }

    /**
     * @brief gets average of both encoders
     * @return encoder average
     */
    public long getEncoderAverage() {
        return (getEncoderLeft() + getEncoderRight()) / 2;
    }
}
